#include "ImageProcessor.h"



/** \brief Detects blobs
*   \param Image matrix, mask matrix
*   \return CBlobResult with found blobs
*
* Method for finding the blobs in image,
* also filters the result deleting blobs that
* are too small to be people
*/
CBlobResult ImageProcessor::blobs(Mat& img, Mat& mask){
	// object that will contain blobs of inputImage
	CBlobResult blobs;

    //Create iplimages from matrices
	IplImage ipl = img;
	IplImage *ipoint;
	ipoint = &ipl;

	IplImage mask_ipl = mask;
	IplImage *mpoint;
	mpoint = &mask_ipl;

	// Extract the blobs using a threshold of 100 in the image
	blobs = CBlobResult(ipoint, mpoint, 100, true );


	// Discard the blobs with less area than 20000 pixels
	blobs.Filter( blobs, B_INCLUDE, CBlobGetArea(), B_GREATER, 20000 );

    CBlobResult newres;
    newres = people(blobs);
	return newres;
}


/** \brief Finds the pose of blobs
*   \param Blobs of an image
*   \return CBlobResult with blobs that are people
*
* Method for finding the people in a blobresult.
* Uses the height/width ratio.
*/
CBlobResult ImageProcessor::people(CBlobResult input){
    CBlob *currentBlob;
    CBlobResult result;
    double ratio, height, width;

    for (int i = 0; i < input.GetNumBlobs(); i++ )
    {
        currentBlob = input.GetBlob(i);
        height = (currentBlob->maxy - currentBlob->miny);
        width = (currentBlob->maxx - currentBlob->minx);

        ratio = height / width;

        //Blobs with this ratio are likely to be people
        if (ratio > 0.7 && ratio < 6.3)
        {
            result.AddBlob(currentBlob);
        }

    }

    return result;
}


/** \brief Finds the people in an image
*   \param Image matrix, mask matrix, depth matrix
*   \return Vector with all found people
*
* Method for finding the people of
* an image. Also draws rectangles around them.
*/
vector<Person> ImageProcessor::findPeople(Mat& img, Mat& mask, Mat& depthImg){
    CBlobResult foundPeople;
    foundPeople = blobs(img, mask);

    CBlob *currentBlob;
    vector<Person> peopleVec;
    Person person;


    for (int i = 0; i < foundPeople.GetNumBlobs(); i++ ){
        currentBlob = foundPeople.GetBlob(i);

        Rect c;
        c.x = currentBlob->minx;
        c.width = currentBlob->maxx - currentBlob->minx;
        c.y = currentBlob->miny;
        c.height = currentBlob->maxy -currentBlob->miny;

        rectangle(img, c.tl(), c.br(), cv::Scalar(255,0,0), 3);

        person = Person(i);
        person.setDistance(calcDistance(currentBlob, depthImg));
        person.setPose(findPose(currentBlob));
        person.setPosition(findPosition(currentBlob));
        person.setGathered(calcDefects(currentBlob));
        person.setSpeed(calcSpeed(currentBlob));
        peopleVec.push_back(person);

    }
    cv::imshow("people",img);

    return peopleVec;
}

/** \brief Finds the distance of blobs
*   \param Pointer to current blob, reference to depth image
*   \return Distance value
*
* Method for finding the distance to
* the centre of a blob using a depth image.
*/

float ImageProcessor::calcDistance(CBlob *blob, Mat& depthImg){
    int x,y;
    float distance;
    x = (blob->maxx - blob->minx)/ 2.0;
    y = (blob->maxy - blob->miny) /2.0;

    distance = (float)depthImg.at<uchar>(x,y);
    return distance;

}

/** \brief Finds the pose of blobs
*   \param Pointer to current blob
*   \return Pose value
*
* Method for finding the pose of
* a blob using the ratio between
* max and min values.
*/
int ImageProcessor::findPose(CBlob *blob){
    int pose = 0;
    double min, ratio;

    //Get ratio between "hand" and "head"
    min = CBlobGetMinYatMaxX()(blob);
    ratio = min/blob->miny;

    /* Translate pose to int:
        0: no pose
        1: T -pose, arms raised
    */
    if (ratio < 4.0){
        pose = 1;
    }

    return pose;
}

/** \brief Finds the position of blobs
*   \param Pointer to current blob
*   \return Position value
*
* Method for finding if people are
* located to the left or right
*/
int ImageProcessor::findPosition(CBlob *blob){


    int position = 0;
    int centreX, centreY;
    centreX = (int)((blob->maxx - blob->minx) / 2.0);
    centreY = (int)((blob->maxy - blob->miny) / 2.0);


    return centreX;

}

/** \brief Calculates speed of blobs
*   \param Pointer to current blob
*   \return Speed value
*
* Method for calculating the speed
* of a blob.
*/
float ImageProcessor::calcSpeed(CBlob *blob){
    float speed = 0.0;

    return speed;
}

/** \brief Mean convex defect depth of a blob
*   \param Pointer to current blob
*   \return Defect depth mean value
*
* Method for pose determination. If there are deep
* convex defects in a blob it is not a gathered object.
*/
float ImageProcessor::calcDefects(CBlob *blob){
    float defectDistance = 0.0;
    static CvMemStorage* storage = 0;
    storage = cvCreateMemStorage(0);
    cvClearMemStorage(storage);


    CvSeq *hull = cvConvexHull2(blob->edges, 0, CV_CLOCKWISE, 0);
    CvSeq *defects = cvConvexityDefects(blob->edges, hull, storage);

    //Loop through found defects
    for(int i = 0; i < (defects ? defects->total : 0); i++ ){
        CvConvexityDefect* d = (CvConvexityDefect*)cvGetSeqElem(defects, i);
        defectDistance += d->depth;
    }

    //Average depth of defects
    defectDistance /= defects->total;
    cvReleaseMemStorage(&storage);
    return defectDistance;
}
